#include <Eigen/Core>
#include <filesystem>
#include <iostream>
#include <memory>
#include <vector>

#include "boost/algorithm/string.hpp"
#include "boost/json.hpp"
#include "boost/json/src.hpp"
#include "common/map_utility.h"
#include "common/utility.h"
#include "filter/map_filter.h"
#include "map_decoder/map_decoder.h"
// #include "fit/dbscan.h"
// #include "fit/rectangle_fitting.h"
#include "perception_visualizer.h"
// #include "track/tracker.h"
// #include "vis/paint.h"
#include "common/map_info.h"

int main(int argc, char const* argv[]) {
  try {
    // get conf file
    std::string conf_json = "../conf/test_conf.json";
    if (!boost::filesystem::exists(conf_json)) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config file is not exists !" << std::endl;
      return (EXIT_FAILURE);
    }

    std::fstream conf_file;
    conf_file.open(conf_json, std::ios_base::in);
    if (!conf_file.is_open()) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Failed to open the config file !" << std::endl;
      return (EXIT_FAILURE);
    }

    // read conf json
    std::string conf_json_str;
    if (conf_file) {
      conf_file.seekg(0, std::ios::end);
      conf_json_str.resize(conf_file.tellg());
      conf_file.seekg(0, std::ios::beg);
      conf_file.read(&conf_json_str[0], conf_json_str.size());
      conf_file.close();
    } else {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Failed to read the config file !" << std::endl;
      return (EXIT_FAILURE);
    }

    boost::json::value conf_json_val = boost::json::parse(conf_json_str);
    boost::json::object conf_json_obj_root = conf_json_val.as_object();

    // get viewer style
    if (!conf_json_obj_root.contains("Viewer_Style")) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config json content does not contain the Viewer_Style !"
                << std::endl;
      return (EXIT_FAILURE);
    }
    boost::json::object viewer_style_obj =
        conf_json_obj_root["Viewer_Style"].as_object();

    // get map json folder path
    if (!conf_json_obj_root.contains("decode_data_root")) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config json content does not contain the decode_data_root !"
                << std::endl;
      return (EXIT_FAILURE);
    }
    std::string decodeDataRoot =
        conf_json_obj_root["decode_data_root"].as_string().c_str();

    // get ground truth map json folder path
    if (!conf_json_obj_root.contains("GroundTruth_Json_Dir_Path")) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config json content does not contain the "
                   "GroundTruth_Json_Dir_Path !"
                << std::endl;
      return (EXIT_FAILURE);
    }
    std::string ground_truth_dir_path =
        conf_json_obj_root["GroundTruth_Json_Dir_Path"].as_string().c_str();

    // get net output timestamp list
    std::string netOutputDataRoot = conf_json_obj_root["net_output_data_root"].as_string().c_str();
    std::vector<std::string> timestamp_list;
    auto const& netOutputDir{std::filesystem::path(netOutputDataRoot) / std::filesystem::path("map_0_cls")};
    if (!std::filesystem::exists(netOutputDir)) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "net output path: " << netOutputDir << " does not exists !"
                << std::endl;
      return (EXIT_FAILURE);
    }
    for (const auto& entry : std::filesystem::directory_iterator(netOutputDir)) {
      if (std::filesystem::is_regular_file(entry.status())) {
        std::string file_path = entry.path().string();
        std::string timestamp = file_path.substr(file_path.rfind('/')+1, 16);
        std::string file_type = file_path.substr(file_path.rfind('.'));
        if (boost::iequals(file_type, ".bin")) {
          timestamp_list.push_back(timestamp);
        }
      }
    }
    std::sort(timestamp_list.begin(), timestamp_list.end());


    auto& perceptionVisualizer{
        autodrive::perception::PerceptionVisualizer::GetInstance()};
    if (!perceptionVisualizer.Init(viewer_style_obj)) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config json exception, perceptionVisualizer init failed!"
                << std::endl;
      return (EXIT_FAILURE);
    }

    // do work
    std::shared_ptr<autodrive::perception::Map> map_src_ptr =
        std::make_shared<autodrive::perception::Map>();
    std::shared_ptr<autodrive::perception::Map> map_filter_ptr =
        std::make_shared<autodrive::perception::Map>();
    std::shared_ptr<autodrive::perception::Map> map_truth_ptr =
        std::make_shared<autodrive::perception::Map>();
    // autodrive::perception::MapJsonLoader map_loader;

    std::shared_ptr<autodrive::perception::Map> map_decoder_ptr =
        std::make_shared<autodrive::perception::Map>();
    autodrive::perception::MapDecoder map_decoder;
    autodrive::perception::MapDecoder::MapDecoderParameter mapDecoderPara{
      .preds_number{2},
      .down_size{4},
      .net_range{-51.2, -51.2, -3, 51.2, 51.2, 5},
      .detection_range{-51.2, -51.2, -3, 51.2, 51.2, 5},
      .voxel_size{0.2, 0.2, 8},
      .type_offset{3},
      .multi_line{0, 0, 0, 0, 0, 0, 0, 0},
      .min_pts_num{6, 6, 6, 6, 6, 6, 6, 6},
      .merge_threshold{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
      .start_cls_threshold{0.65, 0.65, 0.65, 0.45, 0.45, 0.45, 0.45, 0.45},
      .cls_threshold{0.65, 0.65, 0.65, 0.45, 0.45, 0.45, 0.45, 0.45},
      .vec_threshold{45, 15, 15, 45, 30, 30, 30, 45},
      .dis_threshold{0.4, 0.4, 0.4, 0.8, 0.8, 0.8, 0.8, 0.8},
      .keypoint_threshold{0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3},
      .dis_loss_weight{0.3},
    };
    map_decoder.Init(mapDecoderPara);

    std::vector<std::vector<std::int16_t>> preds_cls{8};
    std::vector<std::vector<std::int16_t>> preds_reg{8};
    while (true) {
      for (auto& timestamp : timestamp_list) {
        std::cout << "timestamp : " << timestamp << std::endl;

        for (std::uint8_t cls = 0; cls < 8; ++cls) {
          std::filesystem::path const& clsFile{netOutputDataRoot / std::filesystem::path("map_" + std::to_string(cls) + "_cls") / std::filesystem::path(timestamp + ".bin")};
          std::filesystem::path const& regFile{netOutputDataRoot / std::filesystem::path("map_" + std::to_string(cls) + "_reg") / std::filesystem::path(timestamp + ".bin")};
          autodrive::perception::read_bin_data<std::int16_t>(clsFile.string(), preds_cls.at(cls));
          autodrive::perception::read_bin_data<std::int16_t>(regFile.string(), preds_reg.at(cls));
        }

        // road edge
        map_src_ptr->road_edge.clear();
        map_decoder.Decode<std::int16_t>(preds_cls.at(0).data(), preds_reg.at(0).data(), 0, 9, 12, *map_src_ptr);
        // lane divider
        map_src_ptr->lane_divider.clear();
        map_src_ptr->lane_divider_joint.clear();
        map_decoder.Decode<std::int16_t>(preds_cls.at(1).data(), preds_reg.at(1).data(), 1, 8, 11, *map_src_ptr);
        // center line
        map_src_ptr->center_line.clear();
        map_decoder.Decode<std::int16_t>(preds_cls.at(2).data(), preds_reg.at(2).data(), 2, 9, 10, *map_src_ptr);
        // road marking
        map_src_ptr->road_marking.clear();
        map_decoder.Decode<std::int16_t>(preds_cls.at(3).data(), preds_reg.at(3).data(), 3, 9, 10, *map_src_ptr);
        map_decoder.Decode<std::int16_t>(preds_cls.at(4).data(), preds_reg.at(4).data(), 4, 9, 11, *map_src_ptr);
        map_decoder.Decode<std::int16_t>(preds_cls.at(5).data(), preds_reg.at(5).data(), 5, 9, 12, *map_src_ptr);
        map_decoder.Decode<std::int16_t>(preds_cls.at(6).data(), preds_reg.at(6).data(), 6, 9, 11, *map_src_ptr);
        map_decoder.Decode<std::int16_t>(preds_cls.at(7).data(), preds_reg.at(7).data(), 7, 8, 12, *map_src_ptr);


        std::cout << "map.road_edge.size()" << map_src_ptr->road_edge.size() << std::endl;
        std::cout << "map.lane_divider.size()" << map_src_ptr->lane_divider.size() << std::endl;
        std::cout << "map.center_line.size()" << map_src_ptr->center_line.size() << std::endl;
        std::cout << "map.road_marking.size()" << map_src_ptr->road_marking.size() << std::endl;
        std::cout << "map.lane_divider_joint.size()" << map_src_ptr->lane_divider_joint.size() << std::endl;

        perceptionVisualizer.Enqueue(timestamp,
                                     map_src_ptr.get(), map_filter_ptr.get(),
                                     map_truth_ptr.get());
        std::chrono::milliseconds dura(100);
        std::this_thread::sleep_for(dura);

//        // visulization
//        Paint p;
//        p.Show();
//        std::unique_ptr<autodrive::perception::Tracker> tracker =
//            std::make_unique<autodrive::perception::Tracker>();
//        tracker->Init("../conf");
        // std::cout << "json file = " << json_file << std::endl;
        // float turn_score_threshold = 0.51;   // score的阈值
        // float cross_score_threshold = 0.4;   // score的阈值
        // float nostop_score_threshold = 0.4;  // score的阈值
        // std::vector<Eigen::Vector2f> turn_points;
        // std::vector<Eigen::Vector2f> crosswalk_points;
        // std::vector<Eigen::Vector2f> nostop_points;

        // autodrive::perception::extract_map_roadmarking_data(
        //     *map_src_ptr, turn_points, crosswalk_points, nostop_points,
        //     turn_score_threshold, cross_score_threshold,
        //     nostop_score_threshold);

        // // DBSCAN
        // DBScan<Eigen::Vector2f> dbscan(1.0, 4);
        // // res[0]是噪点   其实是正常点
        // vector<vector<Eigen::Vector2f>> res =
        // dbscan.GetClusters(turn_points); std::cout << "DBSCAN count = " <<
        // res.size() << std::endl; RectangleFitting Fit; vector<cv::Mat> mats;
        // mats.resize(res.size() - 1);
        // for (int i = 1; i < res.size(); ++i) {
        //   const vector<Eigen::Vector2f>& points = res.at(i);
        //   RotatedRect rec;
        //   Fit.Fit(points, rec, mats[i - 1]);
        //   // std::cout << "mat = " << mats[i - 1] << std::endl;
        // }

        // p.Draw(res, mats);
        // // p.Draw(turn_points, res, mats);

        // tracker->Run(*map_src_ptr, pose);
        // p.Draw(*map_src_ptr);
      }
      // std::chrono::milliseconds dura(1000);
      // std::this_thread::sleep_for(dura);
    }
  } catch (const std::exception& e) {
    std::cerr << "run test error, " << e.what() << '\n';
  } catch (...) {
    std::cerr << "unexpected error!!!" << '\n';
  }
  exit(EXIT_SUCCESS);
}
